import java.awt.Color;
import java.awt.Graphics2D;
import java.awt.Polygon;
import java.awt.Rectangle;
import java.awt.Shape;
import java.awt.geom.AffineTransform;
import java.util.ArrayList;


public class Truck {
	
	Color _color;
	
	double _speed = 100;
	double  _angle  = 0;
	
	double _wheelAngle = 0;
	int volanteinfvalue = 0;
	int _wheelRotationSpeed = 120;
	double _x, _y;
	
	int _state = 0;
	
	double _old_x = 0;
	double _old_y = 0;
	
	int _timerIA = 0;
	
	boolean _hasCollided = false;
	
	boolean _start = true;
	
	ArrayList<Rectangle> _obstacleList = null;
	
	Polygon poly = new Polygon();
	Polygon poly2 = new Polygon();
	Sensor sensorFront;
	Sensor sensorBack;
	Sensor sensorLeft_1, sensorLeft_2, sensorLeft_3;
	Sensor sensorRight_1, sensorRight_2, sensorRight_3;
	NeuralNetwork nn;
	int learningEpoch=10000;
	double p_wheel=0.0d;
	double p_truck=0.0d;
	double p_speed=0.0d;

	Polygon p_front = new Polygon();
	Polygon p_back = new Polygon();
	Polygon p_left_1 = new Polygon();
	Polygon p_right_1 = new Polygon();
	Polygon p_left_2 = new Polygon();
	Polygon p_right_2 = new Polygon();
	Polygon p_left_3 = new Polygon();
	Polygon p_right_3 = new Polygon();

	private double inputs[];

	private double bps[];

	private double outputs[];

	public Truck(int x,int y, Color color,ArrayList<Rectangle> listaDeObstaculos) {
		// TODO Auto-generated constructor stub
		_x = x;
		_y = y;
		
		nn = new NeuralNetwork(7, 14, 2);
		
		this._color = color;
		this._obstacleList = listaDeObstaculos;
		
		poly.addPoint(-20, -10);
		poly.addPoint(25, -10);
		poly.addPoint(25, 10);
		poly.addPoint(-20, 10);


		//SENSORES
		p_front.addPoint(30, -10);
		p_front.addPoint(75, -10);
		p_front.addPoint(75, 10);
		p_front.addPoint(30, 10);
		sensorFront = new Sensor(p_front);
		
		p_back.addPoint(-25, 10);
		p_back.addPoint(-70, 10);
		p_back.addPoint(-70, -10);
		p_back.addPoint(-25, -10);
		sensorBack = new Sensor(p_back);
		
		p_left_1.addPoint(30, -15);
		p_left_1.addPoint(65, -15);
		p_left_1.addPoint(65, -50);
		p_left_1.addPoint(30, -50);
		sensorLeft_1 = new Sensor(p_left_1);
		
		p_left_2.addPoint(-20, -15);
		p_left_2.addPoint(25, -15);
		p_left_2.addPoint(25, -50);
		p_left_2.addPoint(-20, -50);
		sensorLeft_2 = new Sensor(p_left_2);
		
		p_left_3.addPoint(-60, -15);
		p_left_3.addPoint(-25, -15);
		p_left_3.addPoint(-25, -50);
		p_left_3.addPoint(-60, -50);
		sensorLeft_3 = new Sensor(p_left_3);
		
		p_right_1.addPoint(30, 15);
		p_right_1.addPoint(65, 15);
		p_right_1.addPoint(65, 50);
		p_right_1.addPoint(30, 50);
		sensorRight_1 = new Sensor(p_right_1);
		
		p_right_2.addPoint(-20, 15);
		p_right_2.addPoint(25, 15);
		p_right_2.addPoint(25, 50);
		p_right_2.addPoint(-20, 50);
		sensorRight_2 = new Sensor(p_right_2);
		
		p_right_3.addPoint(-60, 15);
		p_right_3.addPoint(-25, 15);
		p_right_3.addPoint(-25, 50);
		p_right_3.addPoint(-60, 50);
		sensorRight_3 = new Sensor(p_right_3);
	}
	
	public double get_speed() {
		return _speed;
	}

	public double get_angle() {
		return _angle;
	}

	public double get_wheelAngle() {
		return _wheelAngle;
	}

	public void update(int DiffTime) {
		// TODO Auto-generated method stub
		
		p_wheel = (90+_wheelAngle)/180.0d;
		p_truck = _angle/(Math.PI*2);
		p_speed = (100+_speed)/200.0d;
		
		inputs = new double[] {p_wheel, p_truck, p_speed, 
				sensorFront.checkProximity(_obstacleList, _x, _y, _angle), 
				sensorBack.checkProximity(_obstacleList, _x, _y, _angle), 
				sensorRight_1.checkProximity(_obstacleList, _x, _y, _angle),
				sensorRight_2.checkProximity(_obstacleList, _x, _y, _angle),
				sensorRight_2.checkProximity(_obstacleList, _x, _y, _angle),
				sensorLeft_1.checkProximity(_obstacleList, _x, _y, _angle),
				sensorLeft_2.checkProximity(_obstacleList, _x, _y, _angle),
				sensorLeft_3.checkProximity(_obstacleList, _x, _y, _angle)};
		
		nn.setInput(inputs);
		
		nn.activate();
		
		outputs = nn.getOutput();
		
		if(learningEpoch<=0){
			_wheelAngle = (outputs[0]*180)-90;
			_speed = (outputs[1]*200)-100;
		}
 
		_timerIA+=DiffTime;
		
		_old_x = _x;
		_old_y = _y;
		
		_wheelAngle += volanteinfvalue*_wheelRotationSpeed*DiffTime/1000.0;;
		
		if(_wheelAngle > 90){
			_wheelAngle = 90;
		}
		
		if(_wheelAngle < -90){
			_wheelAngle = -90;
		}
		
		if(_timerIA>100){
			_timerIA = 0;
		}
		
		if(_start){
			_angle += _speed/100.0*((_wheelAngle*Math.PI/2)/90.0f)*DiffTime/1000.0;
			if(_angle < 0){
				_angle = Math.PI*2 + _angle;
			}
			if(_angle > Math.PI*2){
				_angle = _angle-Math.PI*2;
			}
			
			
//			System.out.println("angle "+_angle);
			_x+=Math.cos(_angle)*_speed*DiffTime/1000.0;
			_y+=Math.sin(_angle)*_speed*DiffTime/1000.0;
		}
		
		System.out.println("epoch "+learningEpoch);
		poly2 = new Polygon(poly.xpoints,poly.ypoints,poly.npoints);

		for(int i = 0; i < poly2.npoints;i++){
			double x = poly2.xpoints[i];
			double y = poly2.ypoints[i];
			
			double x2 = x*Math.cos(_angle) - y*Math.sin(_angle);
			double y2 = y*Math.cos(_angle) + x*Math.sin(_angle);
			
			poly2.xpoints[i] = (int)x2;
			poly2.ypoints[i] = (int)y2;
		}		
		
		poly2.translate((int)_x, (int)_y);
		
		
		for(int i = 0; i < _obstacleList.size();i++){
			if(poly2.intersects(_obstacleList.get(i))){
				_x = _old_x;
				_y = _old_y;
				break;
			}
		}
		
		if(learningEpoch>0){
			learningEpoch--;
			p_wheel = (90+_wheelAngle)/180.0d;
			p_truck = _angle/(Math.PI*2);
			p_speed = (100+_speed)/200.0d;
			
			bps = new double[] {p_wheel, p_speed};
			
			nn.applyBackpropagation(bps);
		}

	}

	public void setWheelAngle(double d){
		_wheelAngle = d;
	}

	public void draw(Graphics2D dbg, int XMundo, int YMundo) {
		// TODO Auto-generated method stub
		dbg.setColor(Color.red);
		dbg.draw(poly2);
		
		//dbg.draw((Shape) sensorFront);
		
		dbg.setColor(_color);
		
		AffineTransform trans = dbg.getTransform();
		
		dbg.translate(_x, _y);
		dbg.rotate(_angle);
		
		dbg.setColor(Color.CYAN);
		dbg.draw(p_front);
		dbg.draw(p_back);
		dbg.setColor(Color.red);
		dbg.draw(p_left_1);
		dbg.draw(p_right_1);
		dbg.setColor(Color.orange);
		dbg.draw(p_left_2);
		dbg.draw(p_right_2);
		dbg.setColor(Color.PINK);
		dbg.draw(p_left_3);
		dbg.draw(p_right_3);
		
		
		dbg.setColor(Color.green);
		dbg.drawRect(-20, -10, 40, 20);
		
		dbg.drawRect(20, -5, 5,10);
		
		dbg.setTransform(trans);
	
	}
	
	
	public void steer(int v){
		if(learningEpoch<=0)
			return;
		if(v > 0){
			volanteinfvalue = 1;
		}else if(v < 0){
			volanteinfvalue = -1;
		}else{
			volanteinfvalue = 0;
		}
	}
	
	public void accel(int v){
		if(learningEpoch<=0)
			return;
		if(v > 0){
			_speed = 100;
		}else if(v < 0){
			_speed = -100;
		}else{
			_speed = 0;
		}
	}
	
}
